/*
*****************************************************************
* Copyright All Reserved © 2015-2025 Solonix-Chu
*****************************************************************
*/

#define LOG_TAG "uorb"
#define LOG_LVL LOG_LVL_INFO

#include "uORB.h"
#include <rtdbg.h>
#include <stdbool.h>
#include <rtthread.h>
#include "uorb_node.h"
#ifdef UORB_REGISTER_AS_DEVICE
#include "uorb_device.h"
#endif
#include <string.h>

rt_list_t _orb_node_list;
rt_bool_t _orb_node_list_initialized = RT_FALSE;

// 初始化节点列表
static void orb_node_list_init(void)
{
    if (!_orb_node_list_initialized)
    {
        rt_list_init(&_orb_node_list);
        _orb_node_list_initialized = RT_TRUE;
    }
}

// Determine the data range
static inline bool is_in_range(unsigned left, unsigned value, unsigned right)
{
    if (right > left)
    {
        return (left <= value) && (value <= right);
    }
    else
    { // Maybe the data overflowed and a wraparound occurred
        return (left <= value) || (value <= right);
    }
}

// round up to nearest power of two
// Such as 0 => 1, 1 => 1, 2 => 2 ,3 => 4, 10 => 16, 60 => 64, 65...255 => 128
// Note: When the input value > 128, the output is always 128
static inline rt_uint8_t round_pow_of_two_8(rt_uint8_t n)
{
    if (n == 0)
    {
        return 1;
    }

    // Avoid is already a power of 2
    rt_uint8_t value = n - 1;

    // Fill 1
    value |= value >> 1U;
    value |= value >> 2U;
    value |= value >> 4U;

    // Unable to round-up, take the value of round-down
    if (value == RT_UINT8_MAX)
    {
        value >>= 1U;
    }

    return value + 1;
}

/* 从 meta->o_fields 中解析形如 "@key=123;" 的整型值，未找到返回 -1 */
static int parse_meta_value(const char *fields, const char *key)
{
    if (!fields || !key) return -1;
    char pattern[32];
    rt_snprintf(pattern, sizeof(pattern), "@%s=", key);
    const char *p = strstr(fields, pattern);
    if (!p) return -1;
    p       += rt_strlen(pattern);
    int val  = 0;
    int any  = 0;
    while (*p && *p >= '0' && *p <= '9')
    {
        any = 1;
        val = val * 10 + (*p - '0');
        p++;
    }
    return any ? val : -1;
}

static int get_default_queue_len(const struct orb_metadata_s *meta)
{
    if (!meta || !meta->o_fields) return -1;
    int v = parse_meta_value(meta->o_fields, "queue");
    return (v > 0 && v <= 255) ? v : -1;
}

static int get_default_instances(const struct orb_metadata_s *meta)
{
    if (!meta || !meta->o_fields) return -1;
    int v = parse_meta_value(meta->o_fields, "instances");
    return (v > 0 && v <= ORB_MULTI_MAX_INSTANCES) ? v : -1;
}

orb_node_t *orb_node_create(const struct orb_metadata_s *meta, const rt_uint8_t instance, rt_uint8_t queue_size)
{
    RT_ASSERT(meta != RT_NULL);

    orb_node_list_init(); // Ensure list is initialized

    /* 当调用方给出 queue_size=0 时，使用主题的默认队列长度（若配置），否则回退为1 */
    if (queue_size == 0)
    {
        int defq   = get_default_queue_len(meta);
        queue_size = (defq > 0) ? (rt_uint8_t)defq : 1;
    }

    orb_node_t *node = (orb_node_t *)rt_calloc(sizeof(orb_node_t), 1);
    if (!node)
    {
        return RT_NULL;
    }

    node->meta             = meta;
    node->instance         = instance;
    node->queue_size       = round_pow_of_two_8(queue_size);
    node->generation       = 0;
    node->advertised       = 0;
    node->subscriber_count = 0;
    node->data_valid       = 0;
    node->data             = RT_NULL;
    // Initialize callbacks list
    rt_list_init(&node->callbacks);
    node->dev_min_interval    = 0;
    node->last_read_timestamp = 0;
    node->pending_delete      = RT_FALSE;

    /* 初始化事件通知器 */
    uorb_notifier_init(&node->notifier, "uorb_evt");

    rt_list_insert_after(_orb_node_list.prev, &node->list);

    // 注册设备
    // char name[RT_NAME_MAX];
    // rt_snprintf(name, RT_NAME_MAX, "%s%d", meta->o_name, instance);
    // rt_uorb_register(node, name, 0, RT_NULL);

    return node;
}

rt_err_t orb_node_delete(orb_node_t *node)
{
    if (!node)
    {
        return -RT_ERROR;
    }

    node->advertised = false;

    // 从链表中移除
    rt_list_remove(&node->list);

    // 释放data内存
    if (node->data)
    {
        rt_free(node->data);
        node->data = RT_NULL;
    }

    // 释放事件通知器
    uorb_notifier_deinit(&node->notifier);

    if (node->subscriber_count == 0)
    {
        rt_free(node);
    }
    else
    {
        /* 仍有订阅者：设置延迟删除标记 */
        node->pending_delete = RT_TRUE;
    }

    return RT_EOK;
}


orb_node_t *orb_node_find(const struct orb_metadata_s *meta, int instance)
{
    orb_node_list_init(); // Ensure list is initialized

    // 遍历_node_list
    rt_list_t  *pos;
    orb_node_t *node;

    rt_enter_critical();
    rt_list_for_each(pos, &_orb_node_list)
    {
        node = rt_list_entry(pos, orb_node_t, list);
        if (node->meta == meta && node->instance == instance)
        {
            rt_exit_critical();
            return node;
        }
    }
    rt_exit_critical();


    return RT_NULL;
}

bool orb_node_exists(const struct orb_metadata_s *meta, int instance)
{
    if (!meta)
    {
        return false;
    }

    if (instance < 0 || instance > (ORB_MULTI_MAX_INSTANCES - 1))
    {
        return false;
    }

    orb_node_t *node = orb_node_find(meta, instance);

    if (node && node->advertised)
    {
        return true;
    }

    return false;
}

int orb_node_read(orb_node_t *node, void *data, rt_uint32_t *generation)
{
    RT_ASSERT(node != RT_NULL);

    // 添加对data参数的NULL检查，返回错误而非断言失败
    if (data == RT_NULL)
    {
        return -RT_EINVAL;
    }

    if (!node->data)
    {
        return 0;
    }

    // 当前节点的数据代数（generation），每次写入数据时自增
    const unsigned current_generation = node->generation;
    unsigned       updated_generation = generation ? (*generation) : current_generation;


    if (node->queue_size == 1)
    {
        rt_memcpy(data, node->data, node->meta->o_size);
        updated_generation = current_generation;
    }
    else
    {
        // 多队列场景：
        // 如果订阅者的generation等于当前generation，说明没有新数据，回退一代，防止重复读取
        if (current_generation == updated_generation)
        {
            updated_generation--;
        }

        // 检查updated_generation是否在合法范围内（即数据是否还在队列中，未被覆盖）
        // 如果不在范围内，说明数据已被覆盖，只能读取最早可用的数据
        if (!is_in_range(current_generation - node->queue_size, updated_generation, current_generation - 1))
        {
            updated_generation = current_generation - node->queue_size;
        }

        rt_memcpy(data, node->data + (node->meta->o_size * (updated_generation % node->queue_size)), node->meta->o_size);

        // 读取后，generation自增，表示已消费一条数据
        updated_generation++;
    }

    if (generation)
    {
        *generation = updated_generation;
    }

    return node->meta->o_size;
}

int orb_node_write(orb_node_t *node, const void *data)
{
    RT_ASSERT(node != RT_NULL);

    // 添加对data参数的NULL检查，返回错误而非断言失败
    if (data == RT_NULL)
    {
        return -RT_EINVAL;
    }

    // create buffer
    if (!node->data)
    {
        const size_t size = node->meta->o_size * node->queue_size;
        node->data        = rt_calloc(size, 1);
    }

    // buffer invalid
    if (!node->data)
    {
        return -RT_ERROR;
    }

    rt_enter_critical();

    // copy data to buffer
    rt_memcpy(node->data + (node->meta->o_size * (node->generation % node->queue_size)), data, node->meta->o_size);

    // invoke callbacks
    rt_list_t      *pos;
    orb_callback_t *item;
    rt_list_for_each(pos, &node->callbacks)
    {
        item = rt_list_entry(pos, orb_callback_t, list);
        if (item->call)
        {
            item->call();
        }
    }

    // 通知订阅者（事件）
    uorb_notifier_notify(&node->notifier);

    // mark data valid
    node->data_valid = true;

    // update generation
    node->generation++;

    rt_exit_critical();

    return node->meta->o_size;
}

bool orb_node_ready(orb_subscribe_t *handle)
{
    if (!handle)
    {
        return false;
    }

    if (handle->node)
    {
        return handle->node->advertised;
    }

    handle->node = orb_node_find(handle->meta, handle->instance);

    if (handle->node)
    {
        handle->node->subscriber_count++;
        handle->generation = handle->node->generation;

        return handle->node->advertised;
    }

    return false;
}

